/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.impulserobotics;



import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.camera.AxisCamera;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


 /* This demonstration is not intended to serve as a "starting template" for development of
 * robot code for a team, as there are better templates and examples created specifically
 * for that purpose.  However, teams may find the techniques used in this program to be
 * interesting possibilities for use in their own robot code.
 *
 * The details of the behavior provided by this demonstration are summarized below:
 *
 * Disabled Mode:
 * - Once per second, print (on the console) the number of seconds the robot has been disabled.
 *
 * Autonomous Mode:
 * - Flash the solenoid lights like KITT in Knight Rider
 * - Example code (commented out by default) to drive forward at half-speed for 2 seconds
 *
 * Teleop Mode:
 * - Select between two different drive options depending upon Z-location of Joystick1
 * - When "Z-Up" (on Joystick1) provide "arcade drive" on Joystick1
 * - When "Z-Down" (on Joystick1) provide "tank drive" on Joystick1 and Joystick2
 * - Use Joystick buttons (on Joystick1 or Joystick2) to display the button number in binary on
 *   the solenoid LEDs (Note that this feature can be used to easily "map out" the buttons on a
 *   Joystick.  Note also that if multiple buttons are pressed simultaneously, a "15" is displayed
 *   on the solenoid LEDs to indicate that multiple buttons are pressed.)
 *
 * This code assumes the following connections:
 * - Driver Station:
 *   - USB 1 - The "right" joystick.  Used for either "arcade drive" or "right" stick for tank drive
 *   - USB 2 - The "left" joystick.  Used as the "left" stick for tank drive
 *
 * - Robot:
 *   - Digital Sidecar 1:
 *     - PWM 1/3 - Connected to "left" drive motor(s)
 *     - PWM 2/4 - Connected to "right" drive motor(s)
 *
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class mainRobot extends IterativeRobot {
	// Declare variable for the robot drive system
	RobotDrivePlus m_robotDrive;		// robot will use PWM 1-4 for drive motors
        //First Joystick constructor
      //  Joystick joystick1 = new Joystick(1);
       // double speedFactor = 0.1;
        //allow for switching between forwards and backwards drive. One for ball pickup, one for shooting.
      //  boolean driveBackward = false;
       // boolean directionButtonPressed = false;
       // boolean sonarButtonPressed = false;
       // boolean balanceButtonPressed = false;
        
      //  boolean shootingprimePressed = false;
        boolean aimButtonPressed = false;
        
        AxisCamera camera;
     
        CameraMover camMove;
        Shooter shooter;
        BallLifter ballLift;
        AnalogChannel sonar;
        ADXL345_I2C acc;
        Gyro gyro;
        AnalogChannel fan;
        BridgeLiftingApparatus bla;
       // boolean cameraDetectionOn = false;
        //Sahil adds a compressor
        //Compressor compressor;
        
        
        
        //Ultrasonic Rangefinder
        //Ultrasonic sonar;
        //double sonarMetres;
        
        //Gyro 
        //Gyro gyro;
        //double angle;
        
	int m_dsPacketsReceivedInCurrentSecond;	// keep track of the ds packets received in the current second

	// Declare variables for the two joysticks being used
	Joystick driveStick;			// joystick 1 (arcade stick or right tank stick)
	Joystick camStick;			// joystick 2 (tank left stick)

	//static final int NUM_JOYSTICK_BUTTONS = 16;
	//boolean[] m_rightStickButtonState = new boolean[(NUM_JOYSTICK_BUTTONS+1)];
	//boolean[] m_leftStickButtonState = new boolean[(NUM_JOYSTICK_BUTTONS+1)];

	// Declare variables for each of the eight solenoid outputs
	//static final int NUM_SOLENOIDS = 8;
	//Solenoid[] m_solenoids = new Solenoid[NUM_SOLENOIDS];

//	// drive mode selection
//	static final int UNINITIALIZED_DRIVE = 0;
//	static final int ARCADE_DRIVE = 1;
//	static final int TANK_DRIVE = 2;
	int m_driveMode;

	// Local variables to count the number of periodic loops performed
	int m_autoPeriodicLoops;
	int m_disabledPeriodicLoops;
	int m_telePeriodicLoops;

    /**
     * Constructor for this "BuiltinDefaultCode" Class.
     *
     * The constructor creates all of the objects used for the different inputs and outputs of
     * the robot.  Essentially, the constructor defines the input/output mapping for the robot,
     * providing named objects for each of the robot interfaces.
     */
    public mainRobot() {
        System.out.println("BuiltinDefaultCode Constructor Started\n");

		// Create a robot using standard right/left robot drive on PWMS 1, 2, 3, and #4
		//m_robotDrive = new RobotDrive(1, 3, 2, 4);
//                 SmartDashboard.init();
        SmartDashboard.putDouble("blank", 0.5);
        
                driveStick = new Joystick(1);
		camStick = new Joystick(2);
                 acc = new ADXL345_I2C(1, ADXL345_I2C.DataFormat_Range.k2G);
                m_robotDrive = new RobotDrivePlus(Constants.LEFT_MOTOR_PWM, Constants.RIGHT_MOTOR_PWM, driveStick, acc);
                
                camMove = new CameraMover(Constants.HSERVO_PWM, Constants.VSERVO_PWM);
                
               shooter = new Shooter();
                //limit the motor speed maybe? Sahil
                m_robotDrive.setMaxOutput(Constants.SPEED_FACTOR);
                
                ballLift = new BallLifter();
               sonar = new AnalogChannel(Constants.SONAR_ANALOG_PWM);
               
               bla = new BridgeLiftingApparatus();
                //Start up the air compressor. You have to use a spike relay to power the air compressor.
                //The spike relay must be connected to the port corresponding to 2nd # below:
                //The pressure switch must be connected to the digital input for the 1st # below:
               // compressor = new Compressor(4,2);
               // compressor.start();
             gyro = new Gyro(Constants.GYRO_CHANNEL);
             fan = new AnalogChannel(4);
		m_dsPacketsReceivedInCurrentSecond = 0;

		// Define joysticks being used at USB port #1 and USB port #2 on the Drivers Station
		
                
		// Iterate over all the buttons on each joystick, setting state to false for each
//		int buttonNum = 1;						// start counting buttons at button 1
//		for (buttonNum = 1; buttonNum <= NUM_JOYSTICK_BUTTONS; buttonNum++) {
//			m_rightStickButtonState[buttonNum] = false;
//			m_leftStickButtonState[buttonNum] = false;
//		}
//
//		// Iterate over all the solenoids on the robot, constructing each in turn
//		int solenoidNum = 1;						// start counting solenoids at solenoid 1
//		for (solenoidNum = 0; solenoidNum < NUM_SOLENOIDS; solenoidNum++) {
//			m_solenoids[solenoidNum] = new Solenoid(solenoidNum + 1);
//		}

		// Set drive mode to uninitialized
		m_driveMode = DriveMode.UNINITIALIZED_DRIVE;

		// Initialize counters to record the number of loops completed in autonomous and teleop modes
		m_autoPeriodicLoops = 0;
		m_disabledPeriodicLoops = 0;
		m_telePeriodicLoops = 0;

                    //initialize the camera object. Simple calling getInstance should allow feed to the DS
        camera = AxisCamera.getInstance();
        camera.writeResolution(AxisCamera.ResolutionT.k160x120);
        camera.writeBrightness(50);
                
//                sonar = new Ultrasonic(Constants.PING_CHANNEL, Constants.ECHO_CHANNEL, Ultrasonic.Unit.kMillimeter);
                
		System.out.println("BuiltinDefaultCode Constructor Completed\n");
	}


	/********************************** Init Routines *************************************/

	public void robotInit() {
		// Actions which would be performed once (and only once) upon initialization of the
		// robot would be put here.

		System.out.println("RobotInit() completed.\n");
	}

	public void disabledInit() {
		m_disabledPeriodicLoops = 0;			// Reset the loop counter for disabled mode
//		if(Constants.KITT_MODE_ON)
//                ClearSolenoidLEDsKITT();
        startSec = (int)(Timer.getUsClock() / 1000000.0);
		printSec = startSec + 1;
	}

	public void autonomousInit() {
		m_autoPeriodicLoops = 0;				// Reset the loop counter for autonomous mode
//		ClearSolenoidLEDsKITT();
	}

	public void teleopInit() {
		m_telePeriodicLoops = 0;				// Reset the loop counter for teleop mode
		m_dsPacketsReceivedInCurrentSecond = 0;	// Reset the number of dsPackets in current second
		m_driveMode = DriveMode.UNINITIALIZED_DRIVE;		// Set drive mode to uninitialized
//		if (Constants.KITT_MODE_ON)
//                ClearSolenoidLEDsKITT();
	}

	/********************************** Periodic Routines *************************************/
	static int printSec;
	static int startSec;

	public void disabledPeriodic()  {
		// feed the user watchdog at every period when disabled
		Watchdog.getInstance().feed();

		// increment the number of disabled periodic loops completed
		m_disabledPeriodicLoops++;

		// while disabled, printout the duration of current disabled mode in seconds
		if ((Timer.getUsClock() / 1000000.0) > printSec) {
			System.out.println("Disabled seconds: " + (printSec - startSec));
			printSec++;
		}
	}

	public void autonomousPeriodic() {
		// feed the user watchdog at every period when in autonomous
		Watchdog.getInstance().feed();
                
		m_autoPeriodicLoops++;
                //What we want in autonomous
                //we want to run the belt, because balls will be stored there (or ball)
                //We want to run the shooter motor
                //we want to do something related to the latch??
                //15 seconds worth of code
                
                //Step 1 turn on the shooter
                shooter.rawFire();
                Timer.delay(1.0);
                //turn on the ball lifter
                ballLift.liftMotor.set(Relay.Value.kForward);
                Timer.delay(5);
                //turn off the ball lifter
                ballLift.liftMotor.set(Relay.Value.kOff);
            
                //turn off the shooter
                shooter.endFire();
                Timer.delay(2);
                //move back
                m_robotDrive.arcadeDrive(-0.5, 0);
                Timer.delay(0.3);
                m_robotDrive.arcadeDrive(0,0);
                //jerk back and forth
		
	}

	   public void teleopPeriodic() {
        // feed the user watchdog at every period when in teleop
        Watchdog.getInstance().feed();

        // increment the number of teleop periodic loops completed
        m_telePeriodicLoops++;

        /*
         * Code placed in here will be called only when a new packet of information
         * has been received by the Driver Station.  Any code which needs new information
         * from the DS should go in here
         */

        m_dsPacketsReceivedInCurrentSecond++;					// increment DS packets received

        // put Driver Station-dependent code here

        // Demonstrate the use of the Joystick buttons

//        Solenoid[] firstGroup = new Solenoid[4];
//        Solenoid[] secondGroup = new Solenoid[4];
//        for (int i = 0; i < 4; i++) {
//            firstGroup[i] = m_solenoids[i];
//            secondGroup[i] = m_solenoids[i + 4];
//        }
//        if(Constants.KITT_MODE_ON){
//        DemonstrateJoystickButtons(driveStick, m_rightStickButtonState, "Right Stick", firstGroup);
//        DemonstrateJoystickButtons(camStick, m_leftStickButtonState, "Left Stick ", secondGroup);
//        }
        
        
       // SmartDashboard.putDouble("x",MathUtils.round(acc.getAcceleration(ADXL345_I2C.Axes.kX)*1000)/1000);
        camMove.move(camStick);
        ballLift.update(driveStick);
        m_robotDrive.update();
        shooter.update(this, camStick);
        bla.update(camStick);
        if (camStick.getRawButton(Constants.AIM_BUTTON)!= aimButtonPressed){
            aimButtonPressed = !aimButtonPressed;
            m_robotDrive.setShootingMode(aimButtonPressed);
            camMove.setShootingMode(aimButtonPressed);
                if(aimButtonPressed){
                    m_robotDrive.setStick(camStick);
                }else{
                    m_robotDrive.setStick(driveStick);
                }
        }
       
       // SmartDashboard.putDouble("Gyro Value", MathUtils.round(gyro.getAngle()));
       // double son = sonar.getVoltage()*Constants.SONAR_MAX_DISTANCE;
       // son = MathUtils.round(son*1000);
        //son = son/1000;
        //SmartDashboard.putDouble("Sonar value", son);
        //SmartDashboard.putDouble("fan", MathUtils.round(fan.getVoltage()*1000)/1000);
        
       
                
        
      /*  if((camStick.getRawButton(Constants.SONAR_BUTTON)!= sonarButtonPressed)
                ||(driveStick.getRawButton(Constants.SONAR_BUTTON)!= sonarButtonPressed)){
            
            sonarButtonPressed = !sonarButtonPressed;
            if(sonarButtonPressed){
                sonar.ping();
                Timer.delay(0.5);
               sonarMetres =  sonar.getRangeMM()/1000;
               System.out.println("Sonar sensed object " + sonarMetres + " metres away.");
               }
        }*/
        

        
      /*  if (driveStick.getRawButton(Constants.BALANCE_BUTTON) != balanceButtonPressed){
            
            balanceButtonPressed = !balanceButtonPressed;
            //Get range in centimeters
            double range = sonar.getRangeMM() / 10;
            //Depending on where the sonar is placed, this value will be diff.
            if(balanceButtonPressed && range > 10 ){
                double angle = gyro.getAngle();
                //Not sure about the speed. Rakesh
                m_robotDrive.setMaxOutput(0.15);        
                if (angle < 0){
                    //If I understand correct, there is no driveForwards?
                    m_robotDrive.driveBackwards(driveBackward);
                }
                if (angle > 0){
                    driveBackward = !driveBackward;
                    m_robotDrive.driveBackwards(driveBackward);
                }
                m_robotDrive.setMaxOutput(Constants.SPEED_FACTOR);
            }
        } */
        
       //  Sahil, I haven't looked at the dashboard yet, but is this how
       // you do it? 
          //SmartDashboard.putDouble("Angle", gyro.getAngle());
          //SmartDashboard.putDouble("Range", sonar.getRangeMM());
    }
           

	/**
	 * Clear KITT-style LED display on the solenoids
	 *
	 * Clear the solenoid LEDs used for a KITT-style LED display.
	 */
//	public void ClearSolenoidLEDsKITT() {
//		// Iterate over all the solenoids on the robot, clearing each in turn
//		int solenoidNum = 1;						// start counting solenoids at solenoid 1
//		for (solenoidNum = 0; solenoidNum < NUM_SOLENOIDS; solenoidNum++) {
//			m_solenoids[solenoidNum].set(false);
//		}
//	}

	/**
	 * Generate KITT-style LED display on the solenoids
	 *
	 * This method expects to be called during each periodic loop, with the argument being the
	 * loop number for the current loop.
	 *
	 * The goal here is to generate a KITT-style LED display.  (See http://en.wikipedia.org/wiki/KITT )
	 * However, since the solenoid module has two scan bars, we can have ours go in opposite directions!
	 * The scan bar is written to have a period of one second with six different positions.
	 */
/*	public void SolenoidLEDsKITT(int numloops) {
		final int NUM_KITT_POSITIONS = 6;
		int numloop_within_second = numloops % GetLoopsPerSec();

        // note that the array index values below are zero-based, but solonoid numbers are one based.
/*		if (numloop_within_second == 0) {
			// position 1; solenoids 1 and 8 on
            m_solenoids[0].set(true);
            m_solenoids[7].set(true);
            m_solenoids[1].set(false);
            m_solenoids[6].set(false);
		} else if (numloop_within_second == (GetLoopsPerSec() / NUM_KITT_POSITIONS)) {
			// position 2; solenoids 2 and 7 on
            m_solenoids[1].set(true);
            m_solenoids[6].set(true);
            m_solenoids[0].set(false);
            m_solenoids[7].set(false);
		} else if (numloop_within_second == (GetLoopsPerSec() * 2 / NUM_KITT_POSITIONS)) {
			// position 3; solenoids 3 and 6 on
            m_solenoids[2].set(true);
            m_solenoids[5].set(true);
            m_solenoids[1].set(false);
            m_solenoids[6].set(false);
		} else if (numloop_within_second == (GetLoopsPerSec() * 3 / NUM_KITT_POSITIONS)) {
			// position 4; solenoids 4 and 5 on
            m_solenoids[3].set(true);
            m_solenoids[4].set(true);
            m_solenoids[2].set(false);
            m_solenoids[5].set(false);
		} else if (numloop_within_second == (GetLoopsPerSec() * 4 / NUM_KITT_POSITIONS)) {
			// position 5; solenoids 3 and 6 on
            m_solenoids[2].set(true);
            m_solenoids[5].set(true);
            m_solenoids[3].set(false);
            m_solenoids[4].set(false);
		} else if (numloop_within_second == (GetLoopsPerSec() * 5 / NUM_KITT_POSITIONS)) {
			// position 6; solenoids 2 and 7 on
            m_solenoids[1].set(true);
            m_solenoids[6].set(true);
            m_solenoids[2].set(false);
            m_solenoids[5].set(false);
		}
	}*/

  /*  int GetLoopsPerSec() {
        return 20;
    }

	/**
	 * Demonstrate handling of joystick buttons
	 *
	 * This method expects to be called during each periodic loop, providing the following
	 * capabilities:
	 * - Print out a message when a button is initially pressed
	 * - Solenoid LEDs light up according to joystick buttons:
	 *   - When no buttons pressed, clear the solenoid LEDs
	 *   - When only one button is pressed, show the button number (in binary) via the solenoid LEDs
	 *   - When more than one button is pressed, show "15" (in binary) via the solenoid LEDs
 */
/*	public void DemonstrateJoystickButtons(Joystick currStick,
									boolean[] buttonPreviouslyPressed,
									String stickString,
									Solenoid solenoids[]) {

		int buttonNum = 1;				// start counting buttons at button 1
		boolean outputGenerated = false;		// flag for whether or not output is generated for a button
		int numOfButtonPressed = 0;		// 0 if no buttons pressed, -1 if multiple buttons pressed

		/* Iterate over all the buttons on the joystick, checking to see if each is pressed
		 * If a button is pressed, check to see if it is newly pressed; if so, print out a
		 * message on the console
		 */
/*		for (buttonNum = 1; buttonNum <= NUM_JOYSTICK_BUTTONS; buttonNum++) {
			if (currStick.getRawButton(buttonNum)) {
				// the current button is pressed, now act accordingly...
				if (!buttonPreviouslyPressed[buttonNum]) {
					// button newly pressed; print out a message
					if (!outputGenerated) {
						// print out a heading if no other button pressed this cycle
						outputGenerated = true;
						System.out.println("button pressed:" + stickString);
					}
					System.out.println(" " + buttonNum);
				}
				// remember that this button is pressed for the next iteration
				buttonPreviouslyPressed[buttonNum] = true;

				// set numOfButtonPressed appropriately
				if (numOfButtonPressed == 0) {
					// no button pressed yet this time through, set the number correctly
					numOfButtonPressed = buttonNum;
				} else {
					// another button (or buttons) must have already been pressed, set appropriately
					numOfButtonPressed = -1;
				}
			} else {
				buttonPreviouslyPressed[buttonNum] = false;
			}
		}

		// after iterating through all the buttons, add a newline to output if needed
		if (outputGenerated) {
			System.out.println("\n");
		}

		if (numOfButtonPressed == -1) {
			// multiple buttons were pressed, display as if button 15 was pressed
			DisplayBinaryNumberOnSolenoidLEDs(15, solenoids);
		} else {
			// display the number of the button pressed on the solenoids;
			// note that if no button was pressed (0), the solenoid display will be cleared (set to 0)
			DisplayBinaryNumberOnSolenoidLEDs(numOfButtonPressed, solenoids);
		}
	}


	/**
	 * Display a given four-bit value in binary on the given solenoid LEDs
	 */
/*	void DisplayBinaryNumberOnSolenoidLEDs(int displayNumber, Solenoid[] solenoids) {

		if (displayNumber > 15) {
			// if the number to display is larger than can be displayed in 4 LEDs, display 0 instead
			displayNumber = 0;
		}

		solenoids[3].set( (displayNumber & 1) != 0);
		solenoids[2].set( (displayNumber & 2) != 0);
		solenoids[1].set( (displayNumber & 4) != 0);
		solenoids[0].set( (displayNumber & 8) != 0);
	}*/


}

